#include "Communicate.h"
#include "Motor.h"
#include "Filter.h"

uint8_t numberOfData = 0;
uint8_t length = 0;
int16_t leftSpeed =0;
int16_t rightSpeed =0;
uint8_t commandBuffer[64];

extern float Position_Proportion;
extern float Position_Integral;
extern float Position_Derivative;
extern PID_Union PID_Value;
extern MotorRpm Motor_Rpm;

void Communication_Handler(uint8_t currentData)
{
	static uint8_t  Flag_UART=0;    
		
	//msg_form (hex):  start  +  leagth  +  data

	
	//set expect motor speed
	if( (currentData == 0xAA && Flag_UART==0)  ||  Flag_UART==1)
	{  
			if (Flag_UART==0)   Flag_UART=1;     
		  else 
			{
				if (length==0)     length=currentData;
				else  
				{
					if (numberOfData < length) 
					{
						commandBuffer[numberOfData++] = currentData;
						if (numberOfData==length)
						{
							numberOfData = 0;
							length = 0;
							Flag_UART=0;
							
							//set expect motor rpm value
							Motor_Rpm.LeftExpRpm=*(int16_t *)(commandBuffer);
							Motor_Rpm.RightExpRpm=*(int16_t *)(commandBuffer+2);
							
							
							Filter_Param_Set();
							Set_Motor_Rpm(Motor_Rpm.LeftExpRpm,Motor_Rpm.RightExpRpm);
							
							
						}
					}
				}
			}
	}
	
	//set PID
	if((currentData == 0x55 && Flag_UART==0)  || Flag_UART==2)
	{
			if (Flag_UART==0)   Flag_UART=2;     
		  else 
			{
				if (length==0)     length=currentData;
				else  
				{
					if (numberOfData < length) 
					{
						commandBuffer[numberOfData++] = currentData;
						if (numberOfData==length)
						{
							numberOfData = 0;
							length = 0;
							Flag_UART=0;
							
							//set P I D
							PID_Value.PIDBuf[0]=(*(int16_t *)(commandBuffer));
							PID_Value.PIDBuf[0]=PID_Value.PIDBuf[0]/1000;
							PID_Value.PIDBuf[1]=(*(int16_t *)(commandBuffer+2));
							PID_Value.PIDBuf[1]=PID_Value.PIDBuf[1]/1000;
							PID_Value.PIDBuf[2]=(*(int16_t *)(commandBuffer+4));
							PID_Value.PIDBuf[2]=PID_Value.PIDBuf[2]/1000;
						  PID_Value.PIDBuf[3]=(*(int16_t *)(commandBuffer+6));
							PID_Value.PIDBuf[3]=PID_Value.PIDBuf[3]/1000;
							PID_Value.PIDBuf[4]=(*(int16_t *)(commandBuffer+8));
							PID_Value.PIDBuf[4]=PID_Value.PIDBuf[4]/1000;
							PID_Value.PIDBuf[5]=(*(int16_t *)(commandBuffer+10));
							PID_Value.PIDBuf[5]=PID_Value.PIDBuf[5]/1000;
							
						}
					}
				}
			}
	}
	
	if ((currentData == 0x33 && Flag_UART==0)  || Flag_UART==3)
	{
		if (Flag_UART==0)   Flag_UART=3;   
		else
		{
			if (length==0)     length=currentData;
			else
			{
				if (numberOfData < length) 
				{
					commandBuffer[numberOfData++] = currentData;
					if (numberOfData==length)
					{
						numberOfData = 0;
						length = 0;
						Flag_UART=0;
							
						if ((commandBuffer[0]==0x01) && (commandBuffer[1]==0x01))  Motor_Break(release,release);
						else if ((commandBuffer[0]==0x00) && (commandBuffer[1]==0x01)) Motor_Break(hold,release);
						else if ((commandBuffer[0]==0x01) && (commandBuffer[1]==0x00)) Motor_Break(release,hold);
						else  Motor_Break(hold,hold);
						
					}
				}
			}
		}
	}
	
}
